#include "line_patrol.h"
#include "ti_msp_dl_config.h"

#define PATROLER_DELTA_LENTH (10.5f) // cm

uint8_t line_patroler_get_code()
{
    uint8_t ret = 0;

    if (DL_GPIO_readPins(Line_Patrol_OUT1_PORT, Line_Patrol_OUT1_PIN) & Line_Patrol_OUT1_PIN)
        ret |= 0x01;

    if (DL_GPIO_readPins(Line_Patrol_OUT2_PORT, Line_Patrol_OUT2_PIN) & Line_Patrol_OUT2_PIN)
        ret |= 0x02;

    if (DL_GPIO_readPins(Line_Patrol_OUT3_PORT, Line_Patrol_OUT3_PIN) & Line_Patrol_OUT3_PIN)
        ret |= 0x04;

    if (DL_GPIO_readPins(Line_Patrol_OUT4_PORT, Line_Patrol_OUT4_PIN) & Line_Patrol_OUT4_PIN)
        ret |= 0x08;

    if (DL_GPIO_readPins(Line_Patrol_OUT5_PORT, Line_Patrol_OUT5_PIN) & Line_Patrol_OUT5_PIN)
        ret |= 0x10;

    if (DL_GPIO_readPins(Line_Patrol_OUT6_PORT, Line_Patrol_OUT6_PIN) & Line_Patrol_OUT6_PIN)
        ret |= 0x20;

    if (DL_GPIO_readPins(Line_Patrol_OUT7_PORT, Line_Patrol_OUT7_PIN) & Line_Patrol_OUT7_PIN)
        ret |= 0x40;

    if (DL_GPIO_readPins(Line_Patrol_OUT8_PORT, Line_Patrol_OUT8_PIN) & Line_Patrol_OUT8_PIN)
        ret |= 0x80;

    return ret;
}
/*
@retturn : the current black line center x position in mm ]
           from left to right from 0 to 7*PATROLER_DELTA_LENTH mm
*/

float line_patroler_get_center_x()
{
    uint8_t line_code = line_patroler_get_code();
    int8_t s_idx = -1, e_idx = 0;

    for (int i = 0; i < 8; i++)
        if (line_code & (0x01 << i))
        {
            s_idx = i;
            break;
        }

    for (int i = 7; i >= 0; i--)
        if (line_code & (0x01 << i))
        {
            e_idx = i;
            break;
        }

    float ret = PATROLER_DELTA_LENTH * ((s_idx + e_idx) / 2.0f); // hardware adjustment
    return ret;
}

int8_t line_patroler_get_error()
{
    const static int8_t weight[8] = {-9, -6, -3, -1, 1, 3, 6, 9};
    uint8_t line_code = line_patroler_get_code();
    int8_t ret = 0;

    for (int i = 0; i < 8; i++)
        if (line_code & (0x01 << i))
            ret += weight[i];

    return ret;
}
